#include "costmap_2d.h"

namespace costmap_2d
{

Costmap2D::Costmap2D():
    width_(0), 
    height_(0), 
    resolution_(0.0), 
    origin_x_(0.0), 
    origin_y_(0.0), 
    costmap_(NULL){
    default_value_ = costmap_2d::NO_INFORMATION;
    access_ = new mutex_t();
}

Costmap2D::Costmap2D(const Costmap2D& map):
    costmap_(NULL){
    access_ = new mutex_t();
    *this = map;
}

Costmap2D& Costmap2D::operator=(const Costmap2D& map){
    // check for self assignement
    if (this == &map)
        return *this;
    
    // clean up old data
    deleteMaps();
    width_ = map.width_;
    height_ = map.height_;
    resolution_ = map.resolution_;
    origin_x_ = map.origin_x_;
    origin_y_ = map.origin_y_;
    initMaps(width_, height_);
    // copy the cost map
    memcpy(costmap_, map.costmap_, width_ * height_ * sizeof(unsigned char));

    return *this;
}

Costmap2D::~Costmap2D(){
    deleteMaps();
    delete access_;
}

void Costmap2D::initMaps(unsigned int width, unsigned int height){
    boost::unique_lock<mutex_t> lock(*access_);
    delete[] costmap_;
    costmap_ = new unsigned char[width * height];
}

void Costmap2D::deleteMaps(){
  // clean up data
  boost::unique_lock<mutex_t> lock(*access_);
  delete[] costmap_;
  costmap_ = NULL;
}

void Costmap2D::resizeMap(unsigned int width, unsigned int height, double resolution,
    double origin_x, double origin_y){
    width_ = width;
    height_ = height;
    resolution_ = resolution;
    origin_x_ = origin_x;
    origin_y_ = origin_y;

    initMaps(width, height);

    // reset our maps to have no information
    resetMaps();
}

void Costmap2D::resetMaps(){
  boost::unique_lock<mutex_t> lock(*access_);
  memset(costmap_, default_value_, width_ * height_ * sizeof(unsigned char));
}

void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn){
  boost::unique_lock<mutex_t> lock(*(access_));
  unsigned int len = xn - x0;
  for (unsigned int y = y0 * width_ + x0; y < yn * width_ + x0; y += width_)
    memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const {
    wx = origin_x_ + (mx + 0.5) * resolution_;
    wy = origin_y_ + (my + 0.5) * resolution_;
}

bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
    if (wx < origin_x_ || wy < origin_y_)
        return false;

    mx = (int)((wx - origin_x_) / resolution_);
    my = (int)((wy - origin_y_) / resolution_);

    if (mx < width_ && my < height_)
        return true;

    return false;
}

void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) const{
    if (wx < origin_x_){
        mx = 0;
    }
    else if (wx >= resolution_ * width_ + origin_x_){
        mx = width_ - 1;
    }
    else{
        mx = (int)((wx - origin_x_) / resolution_);
    }

    if (wy < origin_y_){
        my = 0;
    }
    else if (wy >= resolution_ * height_ + origin_y_){
        my = height_ - 1;
    }
    else{
        my = (int)((wy - origin_y_) / resolution_);
    }
}

unsigned int Costmap2D::getIndex(unsigned int mx, unsigned int my) const {
    return my * width_ + mx;
}

void Costmap2D::indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const {
    my = index / width_;
    mx = index - (my * width_);
}

unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const {
    return costmap_[getIndex(mx, my)];
}

void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) {
    costmap_[getIndex(mx, my)] = cost;
}

void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y){
  // project the new origin into the grid
  int cell_ox, cell_oy;
  cell_ox = int((new_origin_x - origin_x_) / resolution_);
  cell_oy = int((new_origin_y - origin_y_) / resolution_);

  // Nothing to update
  if (cell_ox == 0 && cell_oy == 0)
    return;

  // compute the associated world coordinates for the origin cell
  // because we want to keep things grid-aligned
  double new_grid_ox, new_grid_oy;
  new_grid_ox = origin_x_ + cell_ox * resolution_;
  new_grid_oy = origin_y_ + cell_oy * resolution_;

  // To save casting from unsigned int to int a bunch of times
  int size_x = width_;
  int size_y = height_;

  // we need to compute the overlap of the new and existing windows
  int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
  lower_left_x = min(max(cell_ox, 0), size_x);
  lower_left_y = min(max(cell_oy, 0), size_y);
  upper_right_x = min(max(cell_ox + size_x, 0), size_x);
  upper_right_y = min(max(cell_oy + size_y, 0), size_y);

  unsigned int cell_size_x = upper_right_x - lower_left_x;
  unsigned int cell_size_y = upper_right_y - lower_left_y;

  // we need a map to store the obstacles in the window temporarily
  unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];

  // copy the local window in the costmap to the local map
  copyMapRegion(costmap_, lower_left_x, lower_left_y, width_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);

  // now we'll set the costmap to be completely unknown if we track unknown space
  resetMaps();

  // update the origin with the appropriate world coordinates
  origin_x_ = new_grid_ox;
  origin_y_ = new_grid_oy;

  // compute the starting cell location for copying data back in
  int start_x = lower_left_x - cell_ox;
  int start_y = lower_left_y - cell_oy;

  // now we want to copy the overlapping information back into the map, but in its new location
  copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, width_, cell_size_x, cell_size_y);

  // make sure to clean up
  delete[] local_map;
}

unsigned int Costmap2D::cellDistance(double world_dist){
  double cells_dist = max(0.0, ceil(world_dist / resolution_));
  return (unsigned int)cells_dist;
}

unsigned char* Costmap2D::getCharMap() const {
    return costmap_;
}

double Costmap2D::getWidth() const {
    return width_;
}

double Costmap2D::getHeight() const {
    return height_;
}

double Costmap2D::getWidthInMeters() const {
    return (width_ - 1 + 0.5) * resolution_;;
}

double Costmap2D::getHeightInMeters() const {
    return (height_ - 1 + 0.5) * resolution_;
}

double Costmap2D::getOriginX() const {
    return origin_x_;
}

double Costmap2D::getOriginY() const {
    return origin_y_;
}

double Costmap2D::getResolution() const {
    return resolution_;
}

}; // namespace costmap_2d